/**********************************************************************************************
***********************************************************************************************
***  File:			Kalman.h
***	 Author:		José Oniram / Elias Rachid / Jefferson / George Andrew
***	 Contents:		Kalman header file.
***********************************************************************************************
**********************************************************************************************/

#include "listas.h"
#include "Gmatrix.h"
#include <math.h>
#include <cstdlib>

#define alpha1 0.1 
#define alpha2 0.1
#define alpha3 0.1
#define alpha4 0.1
#define sigma_r 0.1
#define sigma_phi 0.1
#define sigma_s 0.01


// Protótipos da Funções presentes no Kalman.cpp
int test (void);
void Kalman_Filter(PGMATRIX Matriz_Mediat, PGMATRIX Matriz_Covt, double controle[], int signature[], list<lista_mapa> *Lista_M, list<lista_sensor> *Lista_S, double d_t);
void Gera_Matriz_Gt(PGMATRIX Matriz_Gt, double controle[], double theta_robo, double d_t);
void Gera_Matriz_Vt(PGMATRIX Matriz_Vt, double controle[], double theta_robo, double d_t);
void Gera_Matriz_Mt(PGMATRIX Matriz_Mt, double controle[]);
void Atualiza_Media_Controle(double media_t[], double controle[], double theta_robo, double d_t);
void Atualiza_Covariancia_Controle(PGMATRIX Matriz_Gt, PGMATRIX Matriz_Vt, PGMATRIX Matriz_Mt, PGMATRIX Matriz_Covt);
void Gera_Matriz_Qt(PGMATRIX Matriz_Qt);
double Gera_Matriz_Ht(PGMATRIX Matriz_Ht, double pos_landmark[], double media_t[]);
void Gera_Matriz_Kt(PGMATRIX Matriz_Kt, PGMATRIX Matriz_Ht, PGMATRIX Matriz_Qt, PGMATRIX Matriz_Covt);
void Atualiza_Media_Covariancia_Sensores(double media_t[], double pos_landmark[], double z_landmark[], PGMATRIX Matriz_Kt, PGMATRIX Matriz_Covt, PGMATRIX Matriz_Ht, PGMATRIX Matriz_I, double rq);

